#!/usr/bin/python
# coding=utf-8
import rospy
from cv_bridge import CvBridge
from sensor_msgs.msg import Image
from lejulib import *
import numpy as np
from motion.motionControl import *
import cv2

direction=1 #1为左转 0为右转
begin_rotation=0
end_rotation=0

GAIT_RANGE = 0.05 #步态的范围
ROTATION_RANGE = 10.0 
ROI = (100, 100) #tuple类型的数组 相当于const的作用无法修改

def triangle(frame): #蓝色
    hsv_image = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)  # 将彩色图像转换为HSV颜色空间

    kernel_2 = np.ones((2, 2), np.uint8)
    kernel_3 = np.ones((3, 3), np.uint8)
    kernel_4 = np.ones((4, 4), np.uint8)

    lower = np.array([0, 163, 9])
    higher = np.array([16, 255, 255])
    mask = cv2.inRange(hsv_image, lower, higher)


    target = cv2.bitwise_and(frame, frame, mask=mask)
    ret, binary = cv2.threshold(mask, 127, 255, cv2.THRESH_BINARY)
    _,contours, hierarchy = cv2.findContours(binary, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
    height = frame.shape[0]
    width = frame.shape[1]
    cv2.rectangle(frame, (height // 2 - 100, 0), (height // 2 + 200, width), (0, 255), 3)
    if len(contours) > 0:
        x, y, w, h = cv2.boundingRect(contours[-1])
        cv2.rectangle(frame, (x, y), (x + w, y + h), (0, 255), 3)
        center_x = x + w // 2
        center_y = y + h // 2
        print(center_x)
        print(center_x)
        if center_x > (height // 2 + 200):
            print("rihgt")
            return "right"
        elif center_x < (height // 2 - 100):
            print("left")
            return "left"
        elif (height // 2 - 100) < center_x <(height // 2 + 200) :
            print("walk")
            return "walk"
    else:
        print("exit object ")
        return "left"
            # cv2.circle(frame, (center_x, center_y), 5, (0, 0, 255), -1)

#调试
        # cv2.imshow('IMG', frame)
        # k = cv2.waitKey(5) & 0xFF
        # if k == 27:
        #     break

def slow_walk(direction, stepnum=1, angle=None):
    """
    :param direction: "forward" ,"backward" or "rotation"
    :param stepnum: int num
    :return:
    """
    array = [0.0, 0.0, 0.0]
    if direction == "forward":
        array[0] = GAIT_RANGE
    elif direction == "backward":
        array[0] = -1 * GAIT_RANGE
    elif direction == "rotation":
        array[2] = ROTATION_RANGE if angle > 0 else -1 * ROTATION_RANGE
        stepnum = int(abs(angle) / ROTATION_RANGE)
    else:
        rospy.logerr("error walk direction")
    for _ in range(stepnum):
        SendGaitCommand(array[0], array[1], array[2])


def move(mean_distance,direction):
    if direction==0:
        angle=-30 #右转30
    else :
        angle=30 #左转30
    print(mean_distance)
    if mean_distance < 150:
        print("离得太近了，我识别不到了。")
        return
    elif mean_distance > 1000:
        print("前方障碍物比较远，我可以走的快一些")
        slow_walk("forward", 4)
        if begin_rotation == 1 :
            end_rotation=1
    elif mean_distance > 500:
        print("我准备往前走了。")
        slow_walk("forward", 1)
    elif mean_distance < 250:
        print("有点近，我需要后退一下")
        slow_walk("backward", 1)
    else:
        print("前方有障碍物，我准备右转30度")
        slow_walk("rotation", angle=angle)
        begin_rotation=1
    WaitForWalkingDone()

def find_color(image):
    cv_image = bridge.imgmsg_to_cv2(image, 'bgr8')  # 16UC1为16位无符号整数
    return triangle(cv_image)
def callback(image):
    # 将ros中的图像信息转换成opencv的图像格式
    cv_image = bridge.imgmsg_to_cv2(image, "16UC1")  # 16UC1为16位无符号整数

    cv_image = np.array(cv_image)  # np.array()函数被用于将从ROS消息转换而来的OpenCV格式图像cv_image转换为NumPy数组进行进一步处理。
    height, width = cv_image.shape  # 依次返回图像的高和宽
    roi_image = cv_image[height / 2 - ROI[1] / 2: height / 2 + ROI[1] / 2,
                width / 2 - ROI[0] / 2: width / 2 + ROI[0] / 2]  # 图像中间框出一个长100宽100的图像
    mean_distance = roi_image.mean()  # roi_image为一个np对象 mean为计算平均值
    return mean_distance  # 也为np对象


if __name__ == "__main__":
    rospy.init_node('roban_avoidance')# 发布节点名称
    rospy.Subscriber('terminate_current_process', String, terminate) #订阅节点
    bridge = CvBridge() #cv_bridge是在ROS图像消息和OpenCV图像之间进行转换的一个功能包。
    topic_raw = "/camera/color/image_raw" # 订阅深度摄像头话题
    topic_depth = "/camera/depth/image_rect_raw"
    print SetBodyhubTo_walking(2)
    flag=1
    while not rospy.is_shutdown():#不发布停止指令就一直运行
        flag=1
        while flag:
            direction2=find_color(rospy.wait_for_message(topic_raw,Image))
            if direction2=="left":
                slow_walk("rotation", angle=15)
                WaitForWalkingDone()

            elif direction2=="right":
                slow_walk("rotation", angle=-15)
                WaitForWalkingDone()
            else:
                flag = 0
                WaitForWalkingDone()
                